#include "init.h"

void __attribute__((interrupt, no_auto_psv)) _PWMSpEventMatchInterrupt(void)
{
    // 40khz
    extern struct state_t state;
    extern struct pid_t pid;
    extern struct setup_t setup;
    //extern unsigned int PIDStartCounter;

    _PSEMIE = 0;
    IFS3bits.PSEMIF = 0;

    if (state.mode < STATE_BEMF_COMMUTATION) return;

    if (pid.output>setup.calcBEMF_ON_PROM && state.BEMFmode==0)
    {
        state.BEMFmode=1;
        MA_TRIS(0);
    }
    else
    {
        if (pid.output<setup.calcBEMF_OFF_PROM && state.BEMFmode==1)
        {
            state.BEMFmode=0;
            MA_TRIS(1);
        }
    }

    MDC=pid.output; // done in T3 ISR
    SDC1=pid.output;

    // Trigger for current?
    STRIG1 = ((pid.output/3)+250);

    if (state.BEMFmode == 1)
    {
        if (pid.output>setup.calcSMP_BEMF_ON_LIM)
        {
            TRIG1=setup.calcSMP_BEMF_ON_LIM - setup.calcSMP_BEMF_ON;
        }
        else
        {
            TRIG1=(pid.output-setup.calcSMP_BEMF_ON);
        }
    }
    else
    {
        TRIG1=setup.calcSMP_BEMF_OFF;
    }
}
